#include "ModelBoundingVolume.h"
#include "Cooking.h"
#include "../Stream.h"

ModelBoundingVolume::ModelBoundingVolume(NxPhysicsSDK *sdk) {
	this->sdk = sdk;
	this->errLog = NULL;
}

ModelBoundingVolume::ModelBoundingVolume(NxPhysicsSDK *sdk, Log *log) {
	this->sdk = sdk;
	this->errLog = log;
}

ModelBoundingVolume::~ModelBoundingVolume() {
	if(this->shapeDescPointer != NULL) {
		delete shapeDescPointer;
	}
}

void ModelBoundingVolume::CalcBoundingVolume(SHAPETYPE shapetype, const ModelAiParamInfo &modelAiInfo, aiMatrix4x4 complNodeTransf, float modelScaleFactor) {
	this->modelShapeType = shapetype;

	if(this->modelShapeType == BOX_SHAPE) {
		bool success = CalcBoxShape(modelAiInfo, complNodeTransf, this->shapeDescPointer, modelScaleFactor);
		if(this->errLog != NULL) {
			if(success) {
				this->errLog->LogDebug(StringBuilder() << "BOX_SHAPE calculated (" << std::string(modelAiInfo.node->mName.data) << ").");
			} else {
				this->errLog->LogError("BOX_SHAPE couln't be calculated.");
			}
		}
	} else if(this->modelShapeType == TRIANGLE_MESH_SHAPE) {
		bool success = CalcTriangleMeshShapes(modelAiInfo, complNodeTransf, this->shapeDescPointer, modelScaleFactor);
		if(this->errLog != NULL) {
			if(success)	{
				this->errLog->LogDebug(StringBuilder() << "TRIANGLE_MESH_SHAPE calculated (" << std::string(modelAiInfo.node->mName.data) << ").");
			} else {
				this->errLog->LogError("TRIANGLE_MESH_SHAPE couln't be calculated.");
			}
		}			
	} else if(this->modelShapeType == CONVEX_SHAPE) {
		bool success = CalcConvexShapes(modelAiInfo, complNodeTransf, this->shapeDescPointer, modelScaleFactor);
		if(this->errLog != NULL) {
			if(success)	{
				this->errLog->LogDebug(StringBuilder() << "CONVEX_SHAPE calculated (" << std::string(modelAiInfo.node->mName.data) << ").");
			} else {
				this->errLog->LogError("CONVEX_SHAPE couln't be calculated.");
			}
		}			
	}
}

NxShapeDesc *ModelBoundingVolume::GetShapeDesc() {
	return this->shapeDescPointer;
}

NxBoxShapeDesc *ModelBoundingVolume::IsBoxShape() {
	if(this->modelShapeType == BOX_SHAPE) {
		return (NxBoxShapeDesc*)(this->shapeDescPointer);
	} else {
		return NULL;
	}
}

NxTriangleMeshShapeDesc *ModelBoundingVolume::IsTriangleMeshShape() {
	if(this->modelShapeType == TRIANGLE_MESH_SHAPE) {
		return (NxTriangleMeshShapeDesc*)(this->shapeDescPointer);
	} else {
		return NULL;
	}
}

NxConvexShapeDesc *ModelBoundingVolume::IsConvexShape() {
	if(this->modelShapeType == CONVEX_SHAPE) {
		return (NxConvexShapeDesc*)(this->shapeDescPointer);
	} else {
		return NULL;
	}
}

bool ModelBoundingVolume::CalcBoxShape(const ModelAiParamInfo &modelAiInfo, aiMatrix4x4 parentTransformation, NxShapeDesc *&shapeDescPointer, float scalefactor) {

	//const aiScene *scene = modelAiInfo.scene;
	const aiNode *node = modelAiInfo.node;
	const aiMesh *mesh = modelAiInfo.mesh;

	//calc bounding box
	aiVector3D min(0.0f, 0.0f, 0.0f);
	aiVector3D max(0.0f, 0.0f, 0.0f);

	for(unsigned int i = 0; i < mesh->mNumVertices; ++i) {
		aiVector3D *vertex = (&mesh->mVertices[i]);

		if(i == 0) {
			min.x = vertex->x * scalefactor;
			min.y = vertex->y * scalefactor;
			min.z = vertex->z * scalefactor;

			max.x = vertex->x * scalefactor;
			max.y = vertex->y * scalefactor;
			max.z = vertex->z * scalefactor;
			continue;
		}

		if(vertex->x * scalefactor < min.x) min.x = vertex->x * scalefactor;
		if(vertex->y * scalefactor < min.y) min.y = vertex->y * scalefactor;
		if(vertex->z * scalefactor < min.z) min.z = vertex->z * scalefactor;

		if(vertex->x * scalefactor > max.x) max.x = vertex->x * scalefactor;
		if(vertex->y * scalefactor > max.y) max.y = vertex->y * scalefactor;
		if(vertex->z * scalefactor > max.z) max.z = vertex->z * scalefactor;
	}

	float xSize,ySize,zSize;
	xSize = (max.x - min.x) / 2.0f;
	ySize = (max.y - min.y) / 2.0f;
	zSize = (max.z - min.z) / 2.0f;

	NxVec3 centerOfBB(min.x + xSize, min.y + ySize,  min.z + zSize);
	NxBoxShapeDesc *boxShape = new NxBoxShapeDesc();
	boxShape->name = node->mName.data;
	boxShape->dimensions = NxVec3(xSize,ySize,zSize);

	NxMat33 matrix;
	matrix.setRow(0, NxVec3(parentTransformation.a1, parentTransformation.a2, parentTransformation.a3));
	matrix.setRow(1, NxVec3(parentTransformation.b1, parentTransformation.b2, parentTransformation.b3));
	matrix.setRow(2, NxVec3(parentTransformation.c1, parentTransformation.c2, parentTransformation.c3));
	
	boxShape->localPose.M = matrix;
	if(strcmp(modelAiInfo.modelfileExtension.c_str(), ".obj") == 0/*no hierarchial data*/) {
		NxVec3 newCenter(0.0f, 0.0f, 0.0f);
		matrix.multiply(centerOfBB,newCenter);
		boxShape->localPose.t = newCenter;
	} else {
		boxShape->localPose.t = NxVec3(parentTransformation.a4, parentTransformation.b4, parentTransformation.c4);
	}
	shapeDescPointer = boxShape;

	return true;
}

bool ModelBoundingVolume::CalcTriangleMeshShapes(const ModelAiParamInfo &modelAiInfo, aiMatrix4x4 parentTransformation, NxShapeDesc *&shapeDescPointer, float scalefactor) {
	//const aiScene *scene = modelAiInfo.scene;
	const aiNode *node = modelAiInfo.node;
	const aiMesh *mesh = modelAiInfo.mesh;

	NxVec3* triangleMeshVertices = new NxVec3[mesh->mNumVertices];
	for (unsigned int t = 0; t < mesh->mNumVertices; ++t) {
		aiVector3D vertice = mesh->mVertices[t];
		triangleMeshVertices[t] = NxVec3(vertice.x * scalefactor, vertice.y * scalefactor, vertice.z * scalefactor);
	}

	NxU32 *triangleMeshFaces = new NxU32[mesh->mNumFaces*3];
	for (unsigned int t = 0; t < mesh->mNumFaces; ++t) {
		const struct aiFace* face = &mesh->mFaces[t];
		for(unsigned int i = 0; i < 3; i++) {
			int index = face->mIndices[i];
			triangleMeshFaces[3*t+i] = index;
		}
	}

	NxTriangleMeshDesc triangleMeshDesc;
	triangleMeshDesc.numVertices				= mesh->mNumVertices;
	triangleMeshDesc.numTriangles				= mesh->mNumFaces;
	triangleMeshDesc.pointStrideBytes			= sizeof(NxVec3);
	triangleMeshDesc.triangleStrideBytes		= 3*sizeof(NxU32);
	triangleMeshDesc.points						= triangleMeshVertices;
	triangleMeshDesc.triangles					= triangleMeshFaces;							
	triangleMeshDesc.flags						= 0;	

	Cooking::InitCooking();
	MemoryWriteBuffer buf;
	if(Cooking::CookTriangleMesh(triangleMeshDesc, buf)) {
		MemoryReadBuffer readBuffer(buf.data);
		NxTriangleMeshShapeDesc *triangleMeshShape = new NxTriangleMeshShapeDesc();
		triangleMeshShape->meshData = sdk->createTriangleMesh(readBuffer);
		triangleMeshShape->name = node->mName.data;
		shapeDescPointer = triangleMeshShape;

		NxMat33 matrix;
		matrix.setRow(0, NxVec3(parentTransformation.a1, parentTransformation.a2, parentTransformation.a3));
		matrix.setRow(1, NxVec3(parentTransformation.b1, parentTransformation.b2, parentTransformation.b3));
		matrix.setRow(2, NxVec3(parentTransformation.c1, parentTransformation.c2, parentTransformation.c3));

		NxVec3 pos(parentTransformation.a4, parentTransformation.b4, parentTransformation.c4);
		shapeDescPointer->localPose.M = matrix;
		shapeDescPointer->localPose.t = pos;
	} else {
		return false;
	}
	
	delete[] triangleMeshVertices;
	delete[] triangleMeshFaces;

	return true;
}

bool ModelBoundingVolume::CalcConvexShapes(const ModelAiParamInfo &modelAiInfo, aiMatrix4x4 parentTransformation, NxShapeDesc *&shapeDescPointer, float scalefactor) {
	//const aiScene *scene = modelAiInfo.scene;
	const aiNode *node = modelAiInfo.node;
	const aiMesh *mesh = modelAiInfo.mesh;

	NxVec3* convexMeshVertices = new NxVec3[mesh->mNumVertices];
	for (unsigned int t = 0; t < mesh->mNumVertices; ++t) {
		aiVector3D vertice = mesh->mVertices[t];
		convexMeshVertices[t] = NxVec3(vertice.x * scalefactor, vertice.y * scalefactor, vertice.z * scalefactor);
	}

	NxU32 *convexMeshFaces = new NxU32[mesh->mNumFaces*3];
	for (unsigned int t = 0; t < mesh->mNumFaces; ++t) {
		const struct aiFace* face = &mesh->mFaces[t];
		for(unsigned int i = 0; i < 3; i++) {
			int index = face->mIndices[i];
			convexMeshFaces[3*t+i] = index;
		}
	}

	NxConvexMeshDesc convexMeshDesc;
	convexMeshDesc.numVertices				= mesh->mNumVertices;
	convexMeshDesc.numTriangles				= mesh->mNumFaces;
	convexMeshDesc.pointStrideBytes			= sizeof(NxVec3);
	convexMeshDesc.triangleStrideBytes		= 3*sizeof(NxU32);
	convexMeshDesc.points					= convexMeshVertices;
	convexMeshDesc.triangles				= convexMeshFaces;							
	convexMeshDesc.flags					= NX_CF_COMPUTE_CONVEX;

	Cooking::InitCooking();
	MemoryWriteBuffer buf;
	if(Cooking::CookConvexMesh(convexMeshDesc, buf)) {
		MemoryReadBuffer readBuffer(buf.data);
		NxConvexShapeDesc *convexMeshShape = new NxConvexShapeDesc();
		convexMeshShape->meshData = sdk->createConvexMesh(readBuffer);

		NxMat33 matrix;
		matrix.setRow(0, NxVec3(parentTransformation.a1, parentTransformation.a2, parentTransformation.a3));
		matrix.setRow(1, NxVec3(parentTransformation.b1, parentTransformation.b2, parentTransformation.b3));
		matrix.setRow(2, NxVec3(parentTransformation.c1, parentTransformation.c2, parentTransformation.c3));

		NxVec3 pos(parentTransformation.a4, parentTransformation.b4, parentTransformation.c4);
		convexMeshShape->localPose.M = matrix;
		convexMeshShape->localPose.t = pos;
		convexMeshShape->name = node->mName.data;
		shapeDescPointer = convexMeshShape;
	} else {
		return false;
	}

	delete[] convexMeshVertices;
	delete[] convexMeshFaces;

	return true;
}
